
#include "full_cover_path_planner/ccpp_path_process.h"
#include "path_smooth/points_process.h"

#include <unordered_set>

using namespace global_path_planner::path_smooth;

namespace global_path_planner
{
    namespace ccpp
    {

        CCPPPathProcess::CCPPPathProcess(std::shared_ptr<Polygon2D>& poly2d)
                                        :__polygon2d_Ptr(poly2d)
        {
        }

        CCPPPathProcess::~CCPPPathProcess()
        {
        }

        // 根据当前点重排下个清扫区域的路径
        void CCPPPathProcess::sortNextSweepPath(const Point_2 &curr_point, std::vector<Point_2> &next_sweep)
        {
            double min_dist = DBL_MAX;
            std::vector<Point_2>::iterator iter;
            // 判断起始点
            auto cur_it = next_sweep.begin();
            double dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            double dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断第二点
            cur_it = next_sweep.begin() + 1;
            dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断最后一个点
            cur_it = next_sweep.end() - 1;
            dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断倒数第二点
            cur_it = next_sweep.end() - 2;
            dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 若为第二个点，与第一个点进行交换
            if (iter == next_sweep.begin() + 1)
            {
                for (auto it = next_sweep.begin(); it != next_sweep.end(); it += 2)
                {
                    if (it + 1 == next_sweep.end())
                        break;
                    auto tmp_iter = *it;
                    *it = *(it + 1);
                    *(it + 1) = tmp_iter;
                }
            }
            // 若为最后一个，进行逆序
            else if (iter == next_sweep.end() - 1)
            {
                std::reverse(next_sweep.begin(), next_sweep.end());
            }
            // 若为倒数第二个点，先将后面两个点插入到开始处，然后删除最后两个点，最后从第三个点到结束进行逆序
            else if (iter == next_sweep.end() - 2)
            {
                next_sweep.insert(next_sweep.begin(), *(next_sweep.end() - 1));
                next_sweep.insert(next_sweep.begin(), *(next_sweep.end() - 2));
                next_sweep.erase(next_sweep.end() - 2);
                next_sweep.erase(next_sweep.end() - 1);
                std::reverse(next_sweep.begin() + 2, next_sweep.end());
                for (auto it = next_sweep.begin() + 2; it != next_sweep.end(); it += 2)
                {
                    if (it + 1 == next_sweep.end())
                        break;
                    auto tmp_iter = *it;
                    *it = *(it + 1);
                    *(it + 1) = tmp_iter;
                }
            }
        }

        // 移除单个cell中重复的路径
        void CCPPPathProcess::removeCellPathsLessSweepRadius(std::vector<Point_2> &cell_paths, float sweep_radius_2)
        {
            int sz = cell_paths.size();
            if (sz <= 2)
                return;

            auto cur_it = cell_paths.end() - 1 - 3;
            auto next_it = cell_paths.end() - 1 - 1;
            auto s_e_pts = Segment_2(*cur_it, *(cur_it + 1));
            auto n_s_e_pts = Segment_2(*next_it, *(next_it + 1));

            auto min_pt = getPointOfMinDis(s_e_pts, n_s_e_pts, cur_it, next_it);

            if(min_pt.first >= sweep_radius_2) return;

            if(min_pt.second == cur_it){
                // 计算当前路径点到下个路径之间的距离
                pointToLineLessSweepRadius2(n_s_e_pts, sweep_radius_2, cell_paths, cur_it);
            }else if(min_pt.second == next_it){
                // 计算下条路径的点到当前路径之间的距离
                pointToLineLessSweepRadius2(s_e_pts, sweep_radius_2, cell_paths, next_it);
            }
        }

        // 移除相邻的路径小于路径宽度的一半
        void CCPPPathProcess::removeRepeatPathsLessSweepRadius(std::vector<std::vector<Point_2>> &cells_sweeps, float sweep_radius_2)
        {
            int sz = cells_sweeps.size();
            if (sz <= 1)
                return;

            for(auto i = 0; i < sz; i++){
                if(cells_sweeps[i].size() < 2) continue;
                for(auto j = i + 1; j < sz; j++){
                    if(cells_sweeps[j].size() < 2) continue;
                    if(cells_sweeps[i].size() < 2) break;

                    #if 0
                    // removePointToLineLessSweepRadius(cells_sweeps[i], cells_sweeps[j], sweep_radius_2);
                    #endif

                    #if 1
                    for(auto cur_it = cells_sweeps[i].begin(); cur_it != cells_sweeps[i].end();){
                        // std::cout << "\n" << std::endl;
                        if(cells_sweeps[j].size() < 2) break;
                        for(auto next_it = cells_sweeps[j].begin(); next_it != cells_sweeps[j].end();){
                            if(cells_sweeps[i].size() < 2) break;
                            auto n_s_e_pts = Segment_2(*next_it, *(next_it + 1));
                            auto s_e_pts = Segment_2(*cur_it, *(cur_it + 1));

                            // std::cout << " s_e_pts.lenth:" << CGAL::to_double(s_e_pts.squared_length())
                            //           << ", n_s_e_pts.lenth:" << CGAL::to_double(n_s_e_pts.squared_length())
                            //          << ", next to s_e_pts dist:" << std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *next_it)))
                            //          << ", next+1 to s_e_pts dist:" <<  std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *(next_it + 1))))
                            //          << ", cur_it to n_s_e_pts dist:" << std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *cur_it)))
                            //          << ", cur_it+1 to n_s_e_pts dist:" <<  std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it + 1))))
                            //          << std::endl;

                            double min_dis = DBL_MAX;
                            std::vector<Point_2>::iterator iter_it = cells_sweeps[j].end();
                            double next_min_dis1 = std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *next_it)));
                            double next_min_dis2 = std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *(next_it + 1))));
                            double next_min_dis = next_min_dis1 > next_min_dis2 ? next_min_dis2 : next_min_dis1;
                            if(next_min_dis < min_dis) {
                                min_dis = next_min_dis;
                                iter_it = next_it;
                            }

                            double cur_min_dis1 = std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *cur_it)));
                            double cur_min_dis2 = std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it + 1))));
                            double cur_min_dis = cur_min_dis1 > cur_min_dis2 ? cur_min_dis2 : cur_min_dis1;
                            if(cur_min_dis < min_dis) {
                                min_dis = cur_min_dis;
                                iter_it = cur_it;
                            }

                            if(min_dis >= sweep_radius_2) {
                                next_it = next_it + 2;
                                continue;
                            }

                            if(iter_it == cur_it){
                                // 计算当前路径点到下个路径之间的距离
                                pointToLineLessSweepRadius2(n_s_e_pts, sweep_radius_2, cells_sweeps[i], cur_it);
                                if(cur_it == cells_sweeps[i].end()) break;
                            }else if(iter_it == next_it){
                                // 计算下条路径的点到当前路径之间的距离
                                if(!pointToLineLessSweepRadius2(s_e_pts, sweep_radius_2, cells_sweeps[j], next_it)){
                                    next_it = next_it + 2;
                                }
                            }
                        }
                        if(cur_it == cells_sweeps[i].end()){
                            break;
                        }
                        else cur_it += 2;
                    }
                    #endif
                }
            }
        }

        // 更新点到直线的距离小于路径宽度的一半
        bool CCPPPathProcess::pointToLineLessSweepRadius2(Segment_2& seg, float sweep_radius_2,
                                                        std::vector<Point_2>& cell_paths,
                                                        std::vector<Point_2>::iterator& iter){
            auto d1 = std::sqrt(CGAL::to_double(CGAL::squared_distance(seg, *iter)));
            auto d2 = std::sqrt(CGAL::to_double(CGAL::squared_distance(seg, *(iter + 1))));
            if(d1 < sweep_radius_2 && d2 < sweep_radius_2){
                iter = cell_paths.erase(iter);
                iter = cell_paths.erase(iter);
                return true;
            }else if(d1 < sweep_radius_2 && d2 >= sweep_radius_2){
                auto p = getPointOfLine(seg, *iter, *(iter + 1), sweep_radius_2);
                *iter = p;
            }else if(d1 >= sweep_radius_2 && d2 < sweep_radius_2){
                auto p = getPointOfLine(seg, *(iter + 1), *iter, sweep_radius_2);
                *(iter + 1) = p;
            }
            return false;
        }

        // 移除路径在障碍物中
        std::vector<std::vector<Point_2>> CCPPPathProcess::removeObstacleSweepPaths(cv::Mat& img, std::vector<std::vector<Point_2>> &all_sweep_paths)
        {
            std::vector<std::vector<Point_2>> remove_obstacle_paths;
            for (auto i = 0; i < all_sweep_paths.size(); i++)
            {
                std::vector<std::vector<Point_2>> res_sweep_paths;
                for (auto j = 0; j < all_sweep_paths[i].size(); j += 2)
                {
                    std::vector<std::vector<Point_2>> tmp_sweep_paths;
                    if (j != all_sweep_paths[i].size() - 2)
                    {
                        std::vector<Point_2> tmp_sweep_path;
                        tmp_sweep_path.emplace_back(all_sweep_paths[i][j]);
                        tmp_sweep_path.emplace_back(all_sweep_paths[i][j + 1]);
                        tmp_sweep_paths.emplace_back(tmp_sweep_path);
                    }
                    else
                    {
                        // 两点之间增加致密点
                        std::tuple<double, double, double> start = std::tuple<double, double, double>(CGAL::to_double(all_sweep_paths[i][j].x()),
                                                                                                    CGAL::to_double(all_sweep_paths[i][j].y()), 0.);
                        std::tuple<double, double, double> end = std::tuple<double, double, double>(CGAL::to_double(all_sweep_paths[i][j + 1].x()),
                                                                                                    CGAL::to_double(all_sweep_paths[i][j + 1].y()), 0.);
                        std::vector<std::tuple<double, double, double>> cv_paths = points::AddDensePoints(start, end, 1);
                        if (cv_paths.size() <= 0)
                            continue;

                        auto start_it = cv_paths.begin();
                        auto end_it = cv_paths.end() - 1;
                        // 从起点开始找到最近可达的点
                        while ((int)img.at<uchar>(std::get<1>(*start_it), std::get<0>(*start_it)) == 0)
                        {
                            start_it++;
                            if (start_it == cv_paths.end())
                                break;
                        }
                        if (start_it == cv_paths.end())
                            continue;
                        // 从最后点开始找到最近可达的点
                        while ((int)img.at<uchar>(std::get<1>(*start_it), std::get<0>(*start_it)) == 0)
                        {
                            end_it--;
                            if (end_it == cv_paths.begin())
                                break;
                        }
                        if (end_it == cv_paths.begin())
                            continue;
                        // 在起点和终点之间查询是否有不可达的点
                        if (start_it == end_it)
                            continue;
                        auto tmp_start = start_it;
                        for (auto it = start_it; it <= end_it;)
                        {
                            // 添加越界判断
                            if ((int)img.at<uchar>(std::get<1>(*it), std::get<0>(*it)) != 255)
                            {
                                // 统计下连续的黑斑个数
                                auto black_iter = it + 1;
                                while (black_iter <= end_it)
                                {
                                    if ((int)img.at<uchar>(std::get<1>(*black_iter), std::get<0>(*black_iter)) != 255)
                                    {
                                        black_iter++;
                                    }
                                    else
                                        break;
                                }
                                if (black_iter - it > 20)
                                {
                                    std::vector<Point_2> tmp_sweep_path;
                                    tmp_sweep_path.emplace_back(Point_2(std::get<0>(*tmp_start), std::get<1>(*tmp_start)));
                                    tmp_sweep_path.emplace_back(Point_2(std::get<0>(*(it - 1)), std::get<1>(*(it - 1))));
                                    tmp_sweep_paths.emplace_back(tmp_sweep_path);
                                    tmp_start = black_iter;
                                }
                                it = black_iter;
                            }
                            else
                            {
                                it++;
                            }
                        }
                        if (tmp_start < end_it)
                        {
                            std::vector<Point_2> tmp_sweep_path;
                            tmp_sweep_path.emplace_back(Point_2(std::get<0>(*tmp_start), std::get<1>(*tmp_start)));
                            tmp_sweep_path.emplace_back(Point_2(std::get<0>(*end_it), std::get<1>(*end_it)));
                            tmp_sweep_paths.emplace_back(tmp_sweep_path);
                        }
                    }
                    for (auto k = 0; k < tmp_sweep_paths.size(); k++)
                    {
                        if (k == 0)
                        {
                            if (res_sweep_paths.size() == 0)
                            {
                                res_sweep_paths.emplace_back(tmp_sweep_paths[k]);
                            }
                            else
                            {
                                res_sweep_paths.back().emplace_back(tmp_sweep_paths[k][0]);
                                res_sweep_paths.back().emplace_back(tmp_sweep_paths[k][1]);
                            }
                        }
                        else
                        {
                            res_sweep_paths.emplace_back(tmp_sweep_paths[k]);
                        }
                    }
                }
                if (res_sweep_paths.size() <= 0)
                    continue;
                for (auto iter = res_sweep_paths.begin(); iter != res_sweep_paths.end(); iter++)
                {
                    remove_obstacle_paths.emplace_back(*iter);
                }
            }
            return remove_obstacle_paths;
        }

        // 重新cell的索引
        std::deque<int> CCPPPathProcess::sortCellsIndex(std::vector<std::vector<Point_2>> &cells_sweeps, Point_2 &start)
        {
            std::unordered_set<int> index_set;
            std::deque<int> cell_idx_path;
            while (cell_idx_path.size() != cells_sweeps.size())
            {
                int starting_cell_idx = -1;
                double min_dist = DBL_MAX;
                for (size_t i = 0; i < cells_sweeps.size(); i++)
                {
                    if (index_set.find(i) != index_set.end())
                        continue;
                    std::vector<Point_2>::iterator iter;
                    if (cells_sweeps[i].size() <= 1)
                        continue;
                    double dist = getCellSweepMinDist(start, cells_sweeps[i], iter);
                    if (min_dist > dist)
                    {
                        min_dist = dist;
                        starting_cell_idx = i;
                    }
                }
                // 把当前的cell索引添加到索引集中
                if (starting_cell_idx == -1)
                    break;
                doReverseNextSweep(start, cells_sweeps[starting_cell_idx]);
                cell_idx_path.emplace_back(starting_cell_idx);
                index_set.insert(starting_cell_idx);
                start = cells_sweeps[starting_cell_idx].back();
            }
        }

        // 当前点离下个规划区域最短的距离
        double CCPPPathProcess::getCellSweepMinDist(const Point_2 &curr_point,
                                                std::vector<Point_2> &next_sweep,
                                                std::vector<Point_2>::iterator &iter)
        {
            double min_dist = DBL_MAX;
            // 判断起始点
            auto it = next_sweep.begin();
            double dx = CGAL::to_double(it->x()) - CGAL::to_double(curr_point.x());
            double dy = CGAL::to_double(it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断第二点
            it = next_sweep.begin() + 1;
            dx = CGAL::to_double(it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断最后一个点
            it = next_sweep.end() - 1;
            dx = CGAL::to_double(it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断倒数第二点
            it = next_sweep.end() - 2;
            dx = CGAL::to_double(it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            return min_dist;
        }

        // 根据当前点对下一个清扫cell重排序
        void CCPPPathProcess::doReverseNextSweep(const Point_2 &curr_point, std::vector<Point_2> &next_sweep)
        {
            double min_dist = DBL_MAX;
            std::vector<Point_2>::iterator iter;
            // 判断起始点
            auto cur_it = next_sweep.begin();
            double dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            double dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断第二点
            cur_it = next_sweep.begin() + 1;
            dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断最后一个点
            cur_it = next_sweep.end() - 1;
            dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 判断倒数第二点
            cur_it = next_sweep.end() - 2;
            dx = CGAL::to_double(cur_it->x()) - CGAL::to_double(curr_point.x());
            dy = CGAL::to_double(cur_it->y()) - CGAL::to_double(curr_point.y());
            if (min_dist > std::sqrt(dx * dx + dy * dy))
            {
                iter = cur_it;
                min_dist = std::sqrt(dx * dx + dy * dy);
            }
            // 若为第二个点，与第一个点进行交换
            if (iter == next_sweep.begin() + 1)
            {
                for (auto it = next_sweep.begin(); it != next_sweep.end(); it += 2)
                {
                    if (it + 1 == next_sweep.end())
                        break;
                    auto tmp_iter = *it;
                    *it = *(it + 1);
                    *(it + 1) = tmp_iter;
                }
            }
            // 若为最后一个，进行逆序
            else if (iter == next_sweep.end() - 1)
            {
                std::reverse(next_sweep.begin(), next_sweep.end());
            }
            // 若为倒数第二个点，先将后面两个点插入到开始处，然后删除最后两个点，最后从第三个点到结束进行逆序
            else if (iter == next_sweep.end() - 2)
            {
                next_sweep.insert(next_sweep.begin(), *(next_sweep.end() - 1));
                next_sweep.insert(next_sweep.begin(), *(next_sweep.end() - 2));
                next_sweep.erase(next_sweep.end() - 2);
                next_sweep.erase(next_sweep.end() - 1);
                std::reverse(next_sweep.begin() + 2, next_sweep.end());
                for (auto it = next_sweep.begin() + 2; it != next_sweep.end(); it += 2)
                {
                    if (it + 1 == next_sweep.end())
                        break;
                    auto tmp_iter = *it;
                    *it = *(it + 1);
                    *(it + 1) = tmp_iter;
                }
            }
        }

        // 根据当前点调整回字形路径
        std::vector<std::pair<float, float>> CCPPPathProcess::sortLoopPath(std::pair<float, float> &start,
                                                                       std::vector<std::pair<float, float>> loop_path)
        {
            std::vector<std::pair<float, float>> sort_loop_path;
            double dist_Min = DBL_MAX;
            std::vector<std::pair<float, float>>::iterator iter = loop_path.end();
            for (auto it = loop_path.begin(); it != loop_path.end(); it++)
            {
                double dx = start.first - it->first;
                double dy = start.second - it->second;
                if (std::sqrt(dx * dx + dy * dy) < dist_Min)
                {
                    dist_Min = std::sqrt(dx * dx + dy * dy);
                    iter = it;
                }
            }

            sort_loop_path.insert(sort_loop_path.end(), iter, loop_path.end());
            sort_loop_path.insert(sort_loop_path.end(), loop_path.begin(), iter);
            sort_loop_path.push_back(sort_loop_path[0]);

            return sort_loop_path;
        }

        // 删除点到直线的距离小于清扫半径的点集
        void CCPPPathProcess::removePointToLineLessPathWidth(const float &path_width_2,
                                                         std::vector<Point_2> &cur_cell_sweep,
                                                         std::vector<Point_2> &adj_cell_sweep)
        {
            auto cur_it = cur_cell_sweep.end() - 2;
            auto adj_it = adj_cell_sweep.begin();
            auto s_e_pts = Segment_2(*cur_it, *(cur_it + 1));
            auto n_s_e_pts = Segment_2(*adj_it, *(adj_it + 1));
            bool flag = true;
            if (CGAL::to_double(s_e_pts.squared_length()) > CGAL::to_double(n_s_e_pts.squared_length()))
            {
                if (std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *adj_it))) < path_width_2 &&
                    std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *(adj_it + 1)))) < path_width_2)
                {
                    adj_it = adj_cell_sweep.erase(adj_it);
                    adj_it = adj_cell_sweep.erase(adj_it);
                    flag = false;
                }
            }
            else
            {
                if (std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it + 1)))) < path_width_2 &&
                    std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it)))) < path_width_2)
                {
                    cur_it = cur_cell_sweep.erase(cur_it);
                    cur_it = cur_cell_sweep.erase(cur_it);
                    flag = false;
                }
            }
            if (adj_cell_sweep.size() < 2 || cur_cell_sweep.size() < 2)
                return;

            cur_it = cur_cell_sweep.end() - 2;
            adj_it = adj_cell_sweep.end() - 2;
            s_e_pts = Segment_2(*cur_it, *(cur_it + 1));
            n_s_e_pts = Segment_2(*adj_it, *(adj_it + 1));
            if (CGAL::to_double(s_e_pts.squared_length()) > CGAL::to_double(n_s_e_pts.squared_length()))
            {
                if (std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *adj_it))) < path_width_2 &&
                    std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *(adj_it + 1)))) < path_width_2)
                {
                    adj_it = adj_cell_sweep.erase(adj_it);
                    adj_it = adj_cell_sweep.erase(adj_it);
                    flag = false;
                }
            }
            else
            {
                if (std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it + 1)))) < path_width_2 &&
                    std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it)))) < path_width_2)
                {
                    cur_it = cur_cell_sweep.erase(cur_it);
                    cur_it = cur_cell_sweep.erase(cur_it);
                    flag = false;
                }
            }
            if (adj_cell_sweep.size() < 2 || cur_cell_sweep.size() < 2)
                return;

            if (flag)
            {
                cur_it = cur_cell_sweep.begin();
                adj_it = adj_cell_sweep.begin();
                s_e_pts = Segment_2(*cur_it, *(cur_it + 1));
                n_s_e_pts = Segment_2(*adj_it, *(adj_it + 1));
                if (CGAL::to_double(s_e_pts.squared_length()) > CGAL::to_double(n_s_e_pts.squared_length()))
                {
                    if (std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *adj_it))) < path_width_2 &&
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *(adj_it + 1)))) < path_width_2)
                    {
                        adj_it = adj_cell_sweep.erase(adj_it);
                        adj_it = adj_cell_sweep.erase(adj_it);
                    }
                }
                else
                {
                    if (std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it + 1)))) < path_width_2 &&
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it)))) < path_width_2)
                    {
                        cur_it = cur_cell_sweep.erase(cur_it);
                        cur_it = cur_cell_sweep.erase(cur_it);
                    }
                }
                if (adj_cell_sweep.size() < 2 || cur_cell_sweep.size() < 2)
                    return;

                cur_it = cur_cell_sweep.begin();
                adj_it = adj_cell_sweep.end() - 2;
                s_e_pts = Segment_2(*cur_it, *(cur_it + 1));
                n_s_e_pts = Segment_2(*adj_it, *(adj_it + 1));
                if (CGAL::to_double(s_e_pts.squared_length()) > CGAL::to_double(n_s_e_pts.squared_length()))
                {
                    if (std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *adj_it))) < path_width_2 &&
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(s_e_pts, *(adj_it + 1)))) < path_width_2)
                    {
                        adj_it = adj_cell_sweep.erase(adj_it);
                        adj_it = adj_cell_sweep.erase(adj_it);
                    }
                }
                else
                {
                    if (std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it + 1)))) < path_width_2 &&
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(n_s_e_pts, *(cur_it)))) < path_width_2)
                    {
                        cur_it = cur_cell_sweep.erase(cur_it);
                        cur_it = cur_cell_sweep.erase(cur_it);
                    }
                }
                if (adj_cell_sweep.size() < 2 || cur_cell_sweep.size() < 2)
                    return;
            }
        }

        // 使用二分法获取线段中一点到另一个线段的距离等于规划路径宽度的一半
        // p1表示到line距离小于规划路径宽度的一半
        // p2表示到line距离大于或等于规划路径宽度的一半
        // line的距离比p1和p2构成的线段要长
        Point_2 CCPPPathProcess::getPointOfLine(Segment_2 &line, Point_2 &p1, Point_2 &p2, float sweep_radius_2)
        {
            Point_2 small = p1;
            Point_2 big = p2;
            Point_2 mid = Point_2((CGAL::to_double(small.x()) + CGAL::to_double(big.x())) / 2,
                                  (CGAL::to_double(small.y()) + CGAL::to_double(big.y())) / 2);
            auto dist = std::sqrt(CGAL::to_double(CGAL::squared_distance(line, mid)));
            while (std::abs(dist - sweep_radius_2) > 0.5)
            {
                if (dist > sweep_radius_2)
                {
                    big = mid;
                }
                else if (dist < sweep_radius_2)
                {
                    small = mid;
                }
                mid = Point_2((CGAL::to_double(big.x()) + CGAL::to_double(small.x())) / 2,
                              (CGAL::to_double(big.y()) + CGAL::to_double(small.y())) / 2);
                dist = std::sqrt(CGAL::to_double(CGAL::squared_distance(line, mid)));
            }
            return mid;
        }

        // 获取一条直线距离另一条直线的最小端点
        std::pair<double, std::vector<Point_2>::iterator>
        CCPPPathProcess::getPointOfMinDis(Segment_2& cur_seg, Segment_2& next_seg,
                                        std::vector<Point_2>::iterator& cur_it,
                                        std::vector<Point_2>::iterator& next_it)
        {
            double min_dis = DBL_MAX;
            std::vector<Point_2>::iterator iter_it = cur_it;
            double cur_min_dis = std::sqrt(CGAL::to_double(CGAL::squared_distance(cur_seg, *next_it))) >
                                std::sqrt(CGAL::to_double(CGAL::squared_distance(cur_seg, *(next_it + 1)))) ?
                                std::sqrt(CGAL::to_double(CGAL::squared_distance(cur_seg, *(next_it + 1)))) :
                                std::sqrt(CGAL::to_double(CGAL::squared_distance(cur_seg, *next_it)));
            if(cur_min_dis < min_dis) {
                min_dis = cur_min_dis;
                iter_it = next_it;
            }

            cur_min_dis = std::sqrt(CGAL::to_double(CGAL::squared_distance(next_seg, *cur_it))) >
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(next_seg, *(cur_it + 1)))) ?
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(next_seg, *(cur_it + 1)))) :
                        std::sqrt(CGAL::to_double(CGAL::squared_distance(next_seg, *cur_it)));
            if(cur_min_dis < min_dis) {
                min_dis = cur_min_dis;
                iter_it = cur_it;
            }
            return std::make_pair(min_dis, iter_it);
        }

    } // namespace ccpp
} // namespace global_path_planner